
#include <math.h>
#include <stdio.h>
#include <string.h>

#include "Core.h"
#include "Aabb2.h"
#include "Vector4.h"
#include "Renderer.h"
#include "Texture2.h"
#include "WindowManager.h"
#include "ConvexPolygon.h"
#include "Disc.h"
#include "PinJoint.h"
#include "SpoolJoint.h"
#include "TrackPinJoint.h"
#include "World.h"

#include "DebugRender.h"

using namespace p2;

static boost::shared_ptr<World> s_world;
static boost::shared_ptr<Window> s_window;
static boost::shared_ptr<Renderer> s_renderer;
static real s_muStatic = 0.1f;
static real s_muKinetic = 0.03f;
static real s_Cr = 0.f;

/*
 *
 */
static void callbackRenderObject(Renderer& renderer, uint32 type, RenderObject& object, Matrix4<real>& modelView);
static void onLButtonDown(Window& win, int32 x, int32 y);
static void onLButtonUp(Window& win, int32 x, int32 y);
static void onRButtonDown(Window& win, int32 x, int32 y);
static void onRButtonUp(Window& win, int32 x, int32 y);
static void onKeyDown(Window& win, int32 k);
static void onKeyUp(Window& win, int32 k);
static void onCursorMove(Window& win, int32 x, int32 y);

//
    BOOL _viewInitialised;

    PinJoint* _touchJoint = NULL;
    
    Vector2<real> _worldOffset;
    Vector2<real> _worldScale;

    bool _dragWorld = false;
    Vector2<real> _dragScreenStart;
    Vector2<real> _dragWorldOffset;

    Texture2* _woodBox1Texture;
    Texture2* _metalBox1Texture;
    Texture2* _iceBox1Texture;
    Texture2* _stoneBox1Texture;
    
    Texture2* _woodTri1Texture;
    Texture2* _metalTri1Texture;
    Texture2* _iceTri1Texture;
    Texture2* _stoneTri1Texture;

    Texture2* _woodDisc1Texture;
    Texture2* _metalDisc1Texture;
    Texture2* _iceDisc1Texture;
    Texture2* _stoneDisc1Texture;

    Texture2* _ropeHorizontal1Texture;
    Texture2* _ropeVertical1Texture;

    Texture2* _debugBoxTexture;
    Texture2* _debugDiscTexture;
    Texture2* _debugTriTexture;
    Texture2* _debugInfMassTexture;
//

bool updateWindow();
void setupGL();
//void tearDownGL();

void updateWorldMatrix();
Vector2<real> screenToWorld(Vector2<real> p);

void initWorld();
void render();
void cursorTest();
void entityTest();
void worldBoundary();
bool loadShaders();
void loadTextures();
void addRenderObjects();
void randomlyPositionedObjects(uint32 count);
void bridge(Vector2<real> position, real width, uint32 segments,
          real segmentHeight, real segmentGap, real segmentMass);
void spoolJointTest();
void ratchetWheel(Vector2<real> x, real radius, int32 numTeeth, real toothHeight, real motorW);
void trackPinJointTest();
void springPinJointTest();
void motorPinJointTest();
void car(Vector2<real> position);

static uint32 s_winX = 0;
static uint32 s_winY = 0;
static uint32 s_worldWidth = 2000;
static uint32 s_worldHeight = 2000;
#ifdef DEBUG
static uint32 s_winW = 800;
static uint32 s_winH = 600;
static bool s_fullscreen = false;
#else
static uint32 s_winW = 1440;
static uint32 s_winH = 900;
static bool s_fullscreen = true;
#endif

int main(int /*argc*/, char** /*argv*/)
{
	try {
		//ASSERTLOG(false, "this IS AN ASSERT");

		WindowManager wman;
		WindowCreateStruct cs;
		cs._name = "glTest";
		cs._worldBound.min() = Vector2<real>::ZERO;
		cs._worldBound.max() = Vector2<real>((real) s_worldWidth, (real) s_worldHeight);
		cs._winX = s_winX;
		cs._winY = s_winY;
		cs._winW = s_winW;
		cs._winH = s_winH;
		cs._unique = "ds2-test";
		cs._fullscreen = s_fullscreen;
		cs._onLButtonDown = onLButtonDown;
		cs._onLButtonUp = onLButtonUp;
		cs._onRButtonDown = onRButtonDown;
		cs._onRButtonUp = onRButtonUp;
		cs._onKeyDown = onKeyDown;
		cs._onKeyUp = onKeyUp;
		cs._onCursorMove = onCursorMove;

		s_window = wman.createWindow(cs);

		setupGL();
		initWorld();

		s_window->show();

		Timer timer(true);
		while (updateWindow())
		{
			updateWorldMatrix();

			real64 elapsed = timer.getElapsedTimeInSec();
			s_world->integrate(elapsed);

			render();

			s_window->swapBuffers();
		}
	}
	catch (std::runtime_error& ex) {
		CONSOLEF("runtime error: %s", ex.what());
		exit(EXIT_FAILURE);
	}
	return 0;
}

void initWorld()
{
    Aabb2<real> worldExtent;
    worldExtent.min() = Vector2<real>::ZERO;
    worldExtent.max() = Vector2<real>((real)s_worldWidth, (real)s_worldHeight);
    s_world = boost::shared_ptr<World>(new World(worldExtent));

    s_world->setTimeStep(0.02);
    s_world->setTimeMultiplier(2.0);
    s_world->setG(Vector2<real>(0.f, -98.f));
    
    _worldOffset = Vector2<real>::ZERO;
    _worldScale = Vector2<real>::ONE;

    springPinJointTest();
    //cursorTest();
	worldBoundary();
	//spoolJointTest();
	ratchetWheel(Vector2<real>(1000, 300), 100, 10, 20, 0);//-0.5f);
    entityTest();
    trackPinJointTest();
    motorPinJointTest();
    //randomlyPositionedObjects(200);
	car(Vector2<real>(500.f, 200.f));
	bridge(Vector2<real>(100.f, 700.f), 1000.f, 12, 24.f, 0.f, 5.f);

    loadTextures();

    addRenderObjects();
}

void setupGL()
{
    // clear gl error
    glGetError();

    glDisable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_ONE, GL_ONE_MINUS_SRC_ALPHA);

    s_renderer = boost::shared_ptr<Renderer>(new Renderer);
	GLint viewport[4];
	glGetIntegerv(GL_VIEWPORT, viewport);
	s_renderer->setProjectionMatrix(Matrix4<real>::makeOrtho(0.f, (real)viewport[2], 0.f, (real)viewport[3], 0.f, 1.f));
	s_renderer->setRenderObjectCallback(callbackRenderObject);
	s_renderer->setClearRgba(Vector4<real>(0.3f, 0.3f, 0.3f, 0.f));

    loadShaders();

	_viewInitialised=true;
}

bool updateWindow()
{
    bool result = true;
    MSG msg;
	while (::PeekMessage(&msg, NULL, 0, 0, PM_REMOVE))
    {
        if (msg.message==WM_QUIT)
        {
            result = false;
        }
        else
        {
            TranslateMessage(&msg); 
            DispatchMessage(&msg); 
        }
    }
    return result;
}

void updateWorldMatrix()
{
	/*
	 * REF: http://www.opengl.org/discussion_boards/showthread.php/177324-Inverse-Orthogonal-Matrix
	 */
	s_renderer->setBaseModelViewMatrix(Matrix4<real>(
		+1.f, 0.f, 0.f, 0.f,
		0.f, +1.f, 0.f, 0.f,
		0.f, 0.f, +1.f, 0.f,
		-_worldOffset.X(), -_worldOffset.Y(), 0.f, +1.f
	));
}

Vector2<real> screenToWorld(Vector2<real> p)
{
	/*
	 * REF: http://www.gamedev.net/topic/543123-invert-one-axis-in-rotation-matrix/
	 */
	Matrix4<real> invBaseMvm(
		+1.f, 0.f, 0.f, 0.f,
		0.f, -1.f, 0.f, 0.f,
		0.f, 0.f, +1.f, 0.f,
		_worldOffset.X(), s_winH-1+_worldOffset.Y(), 0.f, +1.f
	);
	Vector4<real> wp4(Vector4<real>(p) * invBaseMvm);
	return Vector2<real>(wp4.X(), wp4.Y());
}

void renderCursor()
{
	Vector2<real> wcx = screenToWorld(s_window->getCursorPosition());
	RenderVertexVC const vxc[] = {
		{ { wcx.X()-10, wcx.Y() }, { 1.f, 1.f, 1.f, 1.f } },
		{ { wcx.X()+10, wcx.Y() }, { 1.f, 1.f, 1.f, 1.f } },
		{ { wcx.X(), wcx.Y()-10 }, { 1.f, 1.f, 1.f, 1.f } },
		{ { wcx.X(), wcx.Y()+10 }, { 1.f, 1.f, 1.f, 1.f } }
	};
	s_renderer->draw(GL_LINES, vxc, 4);
}

void render()
{
    s_renderer->clear();
    if ( !_viewInitialised ) {
        return;
    }

	s_renderer->render();
    
//#ifdef DEBUG
    s_renderer->switchProgram(PROGRAM_VC);
    s_renderer->setModelView(Matrix4<real>::IDENTITY);

    foreach (Constraint* constraint, s_world->getConstraints()) {
    	renderConstraint(s_renderer, constraint);
    }

    foreach (Entity* entity, s_world->getEntities()) {
    	renderEntity(s_renderer, entity);
    }

    renderContacts(s_renderer, s_world->getCollider());

    /*
     * render the cursor last
     */
    renderGrid(s_renderer, (real)s_worldWidth, (real)s_worldHeight);
//#endif
    renderCursor();
}

void rope()
{
}

void spoolJointTest()
{
	p2Material material(s_muStatic, s_muKinetic, s_Cr);
    Entity* e1 = Entity::create(Vector2<real>(300.f, 300.f), 0.f, NULL);
    s_world->attach(e1);
    ConvexPolygon* p1 = ConvexPolygon::create(50.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    p1->makeBox(25.f, 30.f);
    e1->attach(p1);
    PinJoint* pj = PinJoint::create(
    		Anchor(e1, Vector2<real>::ZERO), Anchor(NULL, Vector2<real>(300.f, 300.f)),
    		0, NULL);
    s_world->attach(pj);
    Entity* e2 = Entity::create(Vector2<real>(280.f, 200.f), 0.f, NULL);
    s_world->attach(e2);
    Disc* d0 = Disc::create(100.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    d0->setRadius(20.f);
    e2->attach(d0);
    SpoolJoint* sj = SpoolJoint::create(
    		Anchor(e1, Vector2<real>(-20, 0)), Anchor(e2, Vector2<real>::ZERO),
    		40, 150, false, NULL);
    s_world->attach(sj);
}

void ratchetWheel(Vector2<real> x, real radius, int32 numTeeth, real toothHeight, real motorW)
{
	p2Material material(s_muStatic, s_muKinetic, s_Cr);

    ConvexPolygon** t = new ConvexPolygon*[numTeeth];

	Entity* w0 = Entity::create(x, 0.f/*-Math<real>::HALF_PI*/, NULL);
	s_world->attach(w0);

	real s = Math<real>::TWO_PI / (real) numTeeth;
	for (int32 i = 0; i < numTeeth; i++) {
		t[i] = ConvexPolygon::create(5.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
		t[i]->addVertex(0, 0);
		real x1 = radius * Math<real>::Cos((real)i * s);
		real y1 = radius * Math<real>::Sin((real)i * s);
		t[i]->addVertex(x1, y1);
		real x2 = (radius+toothHeight) * Math<real>::Cos((real)(i+1) * s);
		real y2 = (radius+toothHeight) * Math<real>::Sin((real)(i+1) * s);
		t[i]->addVertex(x2, y2);
		w0->attach(t[i]);
	}
	Anchor a0(w0, Vector2<real>::ZERO);
	Anchor a1(NULL, x);
	PinJoint* p0 = PinJoint::create(a0, a1, 0.f, NULL);
	//p0->setFriction(0.01);
	//p0->setLimits(0.5f*Math<real>::PI, 1.5f*Math<real>::PI);
	//p0->setSpring(1000.f, 0.f);
	//p0->setMotor(motorW, 0);
	s_world->attach(p0);

	Entity* w1 = Entity::create(Vector2<real>(x[0] - 150.f, x[1] + 125.f), 0.f/*-Math<real>::HALF_PI*/, NULL);
	s_world->attach(w1);
	ConvexPolygon* arm = ConvexPolygon::create(50.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
	arm->makeBox(80, 10);
	w1->attach(arm);
	Disc* pivot = Disc::create(20.f, Vector2<real>(-65, 0), 0.f, material, 0x0000001, NULL);
	pivot->setRadius(20);
	w1->attach(pivot);
	Anchor a2(w1, Vector2<real>(-50, 0));
	Anchor a3(NULL, Vector2<real>(x[0] - 150.f, x[1] + 125.f));
	PinJoint* p1 = PinJoint::create(a2, a3, 0.f, NULL);
	//p1->setSpring(100.f, 0.f);
	s_world->attach(p1);

	PinJoint* p2 = PinJoint::create(Anchor(w0, Vector2<real>::ZERO), Anchor(w1, Vector2<real>(65.f, 0.f)), 0.f, NULL);
	p2->setSpring(500, 0.f);
	s_world->attach(p2);

}

void cursorTest()
{
	p2Material material(s_muStatic, s_muKinetic, s_Cr);
	Entity* e0 = Entity::create(Vector2<real>(400.f, 300.f), 0.3f/*-Math<real>::HALF_PI*/, NULL);
	s_world->attach(e0);
	ConvexPolygon* g = ConvexPolygon::create(INFINITE_MASS, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
	g->makeBox(50.f, 15.f);
	e0->attach(g);
}

void entityTest()
{
	p2Material material(s_muStatic, s_muKinetic, s_Cr);
//	Entity* e0 = Entity::create(Vector2<real>(400.f, 40.f), 0.f/*-Math<real>::HALF_PI*/, NULL);
//	s_world->attach(e0);
//	ConvexPolygon* g = ConvexPolygon::create(INFINITE_MASS, Vector2<real>::ZERO, 0.f, material, NULL);
//	g->makeBox(700.f, 15.f);
//	e0->attach(g);

    /*
     * composite entity test
     */
    Entity* e1 = Entity::create(Vector2<real>(400.f, 80.f), 0.f/*-Math<real>::HALF_PI*/, NULL);
    s_world->attach(e1);
    ConvexPolygon* p0 = ConvexPolygon::create(100.f, Vector2<real>(10.f, -10.f), 1.f, material, 0x0000004, NULL);
    p0->makeBox(15.f, 20.f);
    e1->attach(p0);
    ConvexPolygon* p1 = ConvexPolygon::create(100.f, Vector2<real>(-50.f, 0.f), 0.f, material, 0x0000004, NULL);
    p1->makeBox(30.f, 15.f);
    e1->attach(p1);
	Disc* d0 = Disc::create(100.f, Vector2<real>(0.f, 30.f), 2.f, material, 0x0000001, NULL);
	d0->setRadius(10.f);
	e1->attach(d0);



    e1->setW(-3.f);
//    /*
//     * disc test
//     */
//    Entity* e2 = Entity::create(Vector2<real>(300.f, 300.f), 0.f/*-Math<real>::HALF_PI*/, NULL);
//    s_world->attach(e2);
//    Disc* d0 = Disc::create(100.f, Vector2<real>(0.f, 0.f), 0.f, material, 0x0000002, NULL);
//    d0->setRadius(10.f);
//    e2->attach(d0);
//    e2->setV(Vector2<real>(10.f, 0.f));
//    /*
//     * tri test
//     */
    Entity* e3 = Entity::create(Vector2<real>(100.f, 100.f), 0.f/*-Math<real>::HALF_PI*/, NULL);
    s_world->attach(e3);
    ConvexPolygon* t0 = ConvexPolygon::create(100.f, Vector2<real>(0.f, 0.f), 0.f, material, 0x0000001, NULL);
    t0->makeTriangle(20.f, 20.f);
    e3->attach(t0);
    //e3->addPointForce(Vector2<real>(0.f, -2.f), Vector2<real>(0.f, 30.f));
    //e3->setW(2.f);
    //e3->setV(Vector2<real>(10.f, 0.f));
}

void randomlyPositionedObjects(uint32 count)
{
    p2Material material(s_muStatic, s_muKinetic, s_Cr);
	for (uint32 bi = 0; bi < count; bi++) {
        real x = Math<real>::IntervalRandom(100.f, s_worldWidth - 100.f, 0);
        real y = Math<real>::IntervalRandom(100.f, s_worldHeight - 100.f, 0);
        real xhside = Math<real>::IntervalRandom(8.f, 24.f, 0);
        real yhside = Math<real>::IntervalRandom(8.f, 24.f, 0);

        Entity* e0 = Entity::create(Vector2<real>(x, y), 0.f/*-Math<real>::HALF_PI*/, NULL);
        s_world->attach(e0);
        ConvexPolygon* a = ConvexPolygon::create(+100.f, Vector2<real>::ZERO, 0.f, material, 0x0000004, NULL);
		a->makeBox(yhside, xhside);
		e0->attach(a);
        
//        real type = Math<real>::IntervalRandom(0.f, 4.f);
//        if ( type > 0.f && type < 1.f ) {
//            a->setId() = "wood-box-1";
//        } else if ( type > 1.f && type < 2.f ) {
//            a->id() = "metal-box-1";
//        } else if ( type > 2.f && type < 3.f ) {
//            a->id() = "ice-box-1";
//        } else {
//            a->id() = "stone-box-1";
//        }

		Entity* e1 = Entity::create(Vector2<real>(x-50.f, y), 0.f, NULL);
		s_world->attach(e1);
		ConvexPolygon* b = ConvexPolygon::create(+100.f, Vector2<real>::ZERO, 0.f, material, 0x0000002, NULL);
		b->makeTriangle(yhside, xhside);
		e1->attach(b);
//        if ( type > 0.f && type < 1.f ) {
//            b->id() = "wood-tri-1";
//        } else if ( type > 1.f && type < 2.f ) {
//            b->id() = "metal-tri-1";
//        } else if ( type > 2.f && type < 3.f ) {
//            b->id() = "ice-tri-1";
//        } else {
//            b->id() = "stone-tri-1";
//        }
        Entity* e2 = Entity::create(Vector2<real>(x, y+100.f), 0.f, NULL);
 		s_world->attach(e2);
        Disc* c = Disc::create(+100.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
        c->setRadius(xhside);
        e2->attach(c);
//        if ( type > 0.f && type < 1.f ) {
//            c->id() = "wood-disc-1";
//        } else if ( type > 1.f && type < 2.f ) {
//            c->id() = "metal-disc-1";
//        } else if ( type > 2.f && type < 3.f ) {
//            c->id() = "ice-disc-1";
//        } else {
//            c->id() = "stone-disc-1";
//        }
	}
}

void worldBoundary()
{
	p2Material material(1.f, 1.f, s_Cr);
    real xhside = s_worldWidth * 0.5f;
    real yhside = s_worldHeight * 0.5f;

    Entity* e0 = Entity::create(Vector2<real>(xhside, 25.f), 0.f, NULL);
	s_world->attach(e0);
	ConvexPolygon* bottom = ConvexPolygon::create(INFINITE_MASS, Vector2<real>::ZERO, 0.f, material, 0xffffffff, NULL);
	bottom->makeBox(xhside, 15.f);
	e0->attach(bottom);

    Entity* e3 = Entity::create(Vector2<real>(xhside, s_worldHeight - 25.f), 0.f, NULL);
	s_world->attach(e3);
	ConvexPolygon* top = ConvexPolygon::create(INFINITE_MASS, Vector2<real>::ZERO, 0.f, material, 0xffffffff, NULL);
	top->makeBox(xhside, 15.f);
	e3->attach(top);

    Entity* e1 = Entity::create(Vector2<real>(25.f, yhside), 0.f, NULL);
	s_world->attach(e1);
	ConvexPolygon* left = ConvexPolygon::create(INFINITE_MASS, Vector2<real>::ZERO, 0.f, material, 0xffffffff, NULL);
	left->makeBox(15.f, yhside);
	e1->attach(left);

    Entity* e2 = Entity::create(Vector2<real>(s_worldWidth - 25.f, yhside), 0.f, NULL);
	s_world->attach(e2);
	ConvexPolygon* right = ConvexPolygon::create(INFINITE_MASS, Vector2<real>::ZERO, 0.f, material, 0xffffffff, NULL);
	right->makeBox(15.f, yhside);
	e2->attach(right);
}

void bridge(Vector2<real> position, real width, uint32 segments,
          real segmentHeight, real segmentGap, real segmentMass)
{
    real yhside = segmentHeight * 0.5f;
    real swidth = width / (real)(segments + 2) - segmentGap*(real)(segments + 1);
    real shwidth = swidth * 0.5f;

    p2Material material(1.f, 1.f, s_Cr);


    Entity** s = new Entity*[segments];
    real x = position.X() + shwidth;
    real y = position.Y() - yhside;
    
    Entity* e0 = Entity::create(Vector2<real>(x, y), 0.f, NULL);
	s_world->attach(e0);
	ConvexPolygon* ls = ConvexPolygon::create(INFINITE_MASS, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    ls->makeBox(shwidth, yhside);
    e0->attach(ls);

    x += swidth+segmentGap;
    for ( uint32 i=0; i<segments; i++ ) {
        s[i] = Entity::create(Vector2<real>(x, y), 0.f, NULL);
    	s_world->attach(s[i]);
        ConvexPolygon* p = ConvexPolygon::create(segmentMass, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
        x += swidth+segmentGap;
        p->makeBox(shwidth, yhside);
        //s[i]->id() = "wood-box-1";
        s[i]->attach(p);
    }

    Entity* e1 = Entity::create(Vector2<real>(x, y), 0.f, NULL);
	s_world->attach(e1);
	ConvexPolygon* rs = ConvexPolygon::create(INFINITE_MASS, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    rs->makeBox(shwidth, yhside);
    e1->attach(rs);
    
    Anchor ls0(e0, Vector2<real>(shwidth, -yhside));
    Anchor ls1(s[0], Vector2<real>(-shwidth, yhside));
    PinJoint* lsc = PinJoint::create(ls0, ls1, 0.f, NULL);
    lsc->setSpring(10000.f, 0.1f);
    s_world->attach(lsc);

    for ( uint32 i=1; i < segments; i++ ) {
        Anchor ls0(s[i-1], Vector2<real>(shwidth, yhside));
        Anchor ls1(s[i], Vector2<real>(-shwidth, yhside));
        PinJoint* c = PinJoint::create(ls0, ls1, 0.f, NULL);
        c->setSpring(10000.f, 0.1f);
        s_world->attach(c);
    }

    Anchor rs0(s[segments-1], Vector2<real>(shwidth, yhside));
    Anchor rs1(e1, Vector2<real>(-shwidth, -yhside));
    PinJoint* rsc = PinJoint::create(rs0, rs1, 0.f, NULL);
    rsc->setSpring(10000.f, 0.1f);
    s_world->attach(rsc);

    delete [] s;
}

void car(Vector2<real> position)
{
    p2Material material(1.f, s_muKinetic, s_Cr);

    Entity* e0 = Entity::create(position, 0.f, NULL);
	s_world->attach(e0);
	ConvexPolygon* c = ConvexPolygon::create(+100.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    c->makeBox(100.f, 20.f);
    e0->attach(c);

    //Entity* e1 = Entity::create(Vector2<real>(position.X(), position.Y()+50.f), 0.f, NULL);
    Entity* e1 = Entity::create(Vector2<real>(position.X() - 110.f, position.Y() - 55.f), 0.f, NULL);
	s_world->attach(e1);
    Disc* w0 = Disc::create(+10.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    w0->setRadius(10.f);
    e1->attach(w0);

    Entity* e2 = Entity::create(Vector2<real>(position.X() + 110.f, position.Y() - 55.f), 0.f, NULL);
	s_world->attach(e2);
    Disc* w1 = Disc::create(+10.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    w1->setRadius(10.f);
    e2->attach(w1);

    Anchor cw1a(e0, Vector2<real>(+75.f, -20.f));
    Anchor cw1b(e2, Vector2<real>::ZERO);
    TrackPinJoint* cw1 = TrackPinJoint::create(cw1a, cw1b, Vector2<real>(0, -1.f), 70.f, NULL);
    cw1->setSpring(2500.f, 0.1f, 35.f);
    //cw1->setMotor(2.f, 0.f);
    s_world->attach(cw1);
    //Anchor cw0a(e0, Vector2<real>(0, 20.f));
    Anchor cw0a(e0, Vector2<real>(-75.f, -20.f));
    Anchor cw0b(e1, Vector2<real>::ZERO);
    //SlidingPinJoint* cw0 = SlidingPinJoint::create(cw0a, cw0b, Vector2<real>(0, +1.f), 10.f, NULL);
    TrackPinJoint* cw0 = TrackPinJoint::create(cw0a, cw0b, Vector2<real>(0, -1.f), 70.f, NULL);
    cw0->setSpring(2500.f, 0.1f, 35.f);
    //cw0->setMotor(2.f, 0.f);
    s_world->attach(cw0);
}

void springPinJointTest()
{
    p2Material material(s_muStatic, s_muKinetic, s_Cr);

	Entity* e1 = Entity::create(Vector2<real>(400.f, 250.f), 0.f, NULL);
    s_world->attach(e1);
    ConvexPolygon* a = ConvexPolygon::create(+100.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    a->makeBox(30.f, 50.f);
    e1->attach(a);
	Anchor a0(e1, Vector2<real>::ZERO);
	Anchor a1(NULL, Vector2<real>(400.f, 250.f));
	PinJoint* p1 = PinJoint::create(a0, a1, 0.f, NULL);
	//p1->setFriction(0.01);
	p1->setLimits(0.5f*Math<real>::PI, 1.5f*Math<real>::PI);
	//p1->setSpring(1000.f, 0.f);
	s_world->attach(p1);

    Entity* e2 = Entity::create(Vector2<real>(200.f, 200.f), 0.f, NULL);
    ConvexPolygon* b = ConvexPolygon::create(+20.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    b->makeBox(16.f, 16.f);
    e2->attach(b);
    s_world->attach(e2);

    Anchor a2(e1, Vector2<real>(30.f, 50.f));
	Anchor a3(e2, Vector2<real>(16.f, 16.f));
	PinJoint* p2 = PinJoint::create(a2, a3, 70.f, NULL);
	p2->addFlags(PinJoint::FLAGS_FLEXIBLE_LENGTH);
	//p2->setFriction(0.01);
	//p2->setSpring(10.f, 0.01f);
	s_world->attach(p2);
}

void trackPinJointTest()
{
    p2Material material(s_muStatic, s_muKinetic, s_Cr);

	Entity* e1 = Entity::create(Vector2<real>(600.f, 200.f), 0.f, NULL);
    s_world->attach(e1);
    ConvexPolygon* a = ConvexPolygon::create(+100.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    a->makeBox(50.f, 30.f);
    e1->attach(a);

	Anchor a0(e1, Vector2<real>::ZERO);
	Anchor a1(NULL, Vector2<real>(600.f, 200.f));
	PinJoint* p1 = PinJoint::create(a0, a1, 0.f, NULL);
	p1->setFriction(0.01f);
	//p1->setSpring(100.f, 0.f);
	s_world->attach(p1);

    Entity* e2 = Entity::create(Vector2<real>(600.f, 300.f), 0.f, NULL);
    ConvexPolygon* b = ConvexPolygon::create(+20.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    b->makeBox(16.f, 16.f);
    e2->attach(b);
    e2->setV(Vector2<real>(-1, +.5));
    s_world->attach(e2);
    
    Anchor c0(e1, Vector2<real>(-100.f, 100.f));
    Anchor c1(e2, Vector2<real>(0.f, 0.f));
    TrackPinJoint* sp = TrackPinJoint::create(c0, c1, Vector2<real>(+1.f, 0.f), 200.f, NULL);
    sp->setSpring(500.f, 0.01f, 100.f);
    s_world->attach(sp);
}

void motorPinJointTest()
{
    p2Material material(s_muStatic, s_muKinetic, s_Cr);

	Entity* e1 = Entity::create(Vector2<real>(700.f, 200.f), 0.f, NULL);
    s_world->attach(e1);
    ConvexPolygon* a = ConvexPolygon::create(+100.f, Vector2<real>::ZERO, 0.f, material, 0x0000001, NULL);
    a->makeBox(20.f, 20.f);
    e1->attach(a);

	Anchor a0(e1, Vector2<real>::ZERO);
	Anchor a1(NULL, Vector2<real>(700.f, 200.f));
	PinJoint* p1 = PinJoint::create(a0, a1, 0.f, NULL);
	p1->setMotor(1.f, 0.f);
	p1->setSpring(1000.f, 0.f);
	s_world->attach(p1);

//    Entity* e2 = Entity::create(Vector2<real>(200.f, 200.f), 0.f, NULL);
//    ConvexPolygon* b = ConvexPolygon::create(+20.f, Vector2<real>::ZERO, 0.f, material, NULL);
//    b->makeBox(16.f, 16.f);
//    e2->attach(b);
//    s_world->attach(e2);
//
//    Anchor c0(e1, Vector2<real>(0.f, 50.f));
//    Anchor c1(e2, Vector2<real>(16.f, 16.f));
//    SlidingPinJoint* sp = SlidingPinJoint::create(c0, c1, Vector2<real>(0.f, +1.f), 300.f, NULL);
//    sp->setSpring(50.f, 100.f, 0.001f);
//    s_world->attach(sp);
}

bool loadShaders()
{
    if (!s_renderer->loadShaders(PROGRAM_VC, "Shaders\\shaderVC.vsh", "Shaders\\shaderVC.fsh")) {
        return false;
    }
    if (!s_renderer->loadShaders(PROGRAM_VT, "Shaders\\shaderVT.vsh", "Shaders\\shaderVT.fsh")) {
        return false;
    }
    if (!s_renderer->loadShaders(PROGRAM_VTC, "Shaders\\shaderVTC.vsh", "Shaders\\shaderVTC.fsh")) {
        return false;
    }
    return true;
}

void loadTextures()
{
	_woodBox1Texture = new Texture2();
	_woodBox1Texture->fromFile("Textures\\wood-box-1-64x64.png");
    _metalBox1Texture = new Texture2();
    _metalBox1Texture->fromFile("Textures\\metal-box-1-64x64.png");
    _iceBox1Texture = new Texture2();
    _iceBox1Texture->fromFile("Textures\\ice-box-1-64x64.png");
    _stoneBox1Texture = new Texture2();
    _stoneBox1Texture->fromFile("Textures\\stone-box-1-64x64.png");

    _woodTri1Texture = new Texture2();
    _woodTri1Texture->fromFile("Textures\\wood-tri-1-64x64.png");
    _metalTri1Texture = new Texture2();
    _metalTri1Texture->fromFile("Textures\\metal-tri-1-64x64.png");
    _iceTri1Texture = new Texture2();
    _iceTri1Texture->fromFile("Textures\\ice-tri-1-64x64.png");
    _stoneTri1Texture = new Texture2();
    _stoneTri1Texture->fromFile("Textures\\stone-tri-1-64x64.png");

    _woodDisc1Texture = new Texture2();
    _woodDisc1Texture->fromFile("Textures\\wood-disc-1-64x64.png");
    _metalDisc1Texture = new Texture2();
    _metalDisc1Texture->fromFile("Textures\\metal-disc-1-64x64.png");
    _iceDisc1Texture = new Texture2();
    _iceDisc1Texture->fromFile("Textures\\ice-disc-1-64x64.png");
    _stoneDisc1Texture = new Texture2();
    _stoneDisc1Texture->fromFile("Textures\\stone-disc-1-64x64.png");

    _ropeHorizontal1Texture = new Texture2();
    _ropeHorizontal1Texture->fromFile("Textures\\rope-horizontal-1-64x64.png");
    _ropeVertical1Texture = new Texture2();
    _ropeVertical1Texture->fromFile("Textures\\rope-vertical-1-64x64.png");

//    _debugInfMassTexture = new Texture2();
//    _debugInfMassTexture->fromFile("Textures\\seamless-hazard-1-256x256.png", GL_REPEAT, GL_REPEAT);
    _debugBoxTexture = new Texture2();
    _debugBoxTexture->fromFile("Textures\\box-debug-outline-64x64.png");
//    _debugTriTexture = new Texture2();
//    _debugTriTexture->fromFile("Textures\\tri-debug-outline-64x64.png");
//    _debugDiscTexture = new Texture2();
//    _debugDiscTexture->fromFile("Textures\\disc-debug-outline-64x64.png");
    _debugInfMassTexture = new Texture2();
    _debugInfMassTexture->fromFile("Textures\\seamless-hazard-1-256x256.png", GL_REPEAT, GL_REPEAT);
    //_debugBoxTexture = new Texture2();
    //_debugBoxTexture->fromFile("Textures\\wood-box-1-64x64.png");
    _debugTriTexture = new Texture2();
    _debugTriTexture->fromFile("Textures\\tri-debug-outline-64x64.png");
    _debugDiscTexture = new Texture2();
    _debugDiscTexture->fromFile("Textures\\disc-debug-outline-64x64.png");
}

void addRenderObjects()
{
    Entity::Set::iterator entityIter = s_world->getEntities().begin();
    for ( ; entityIter != s_world->getEntities().end(); ++entityIter ) {
    	Entity* entity = (*entityIter);
        Element::Set::iterator elemIter = entity->getElements().begin();
        for ( ; elemIter != entity->getElements().end(); ++elemIter ) {
        	Element* element = (*elemIter);
			RenderObject obj;
			obj.setUserData(element);
			ConvexPolygon* polygon = ConvexPolygon::cast(element);
			if ( polygon ) {
				switch (polygon->getShapeHint()) {
					case BoxPolygonShape:
//						if ( element->id() == "wood-box-1" ) {
//							obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id, _woodBox1Texture->getId(), _woodBox1Texture->getWidth(), _woodBox1Texture->getHeight());
//						} else if ( element->id() == "metal-box-1" ) {
//							obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id, _metalBox1Texture->getId(), _metalBox1Texture->getWidth(), _metalBox1Texture->getHeight());
//						} else if ( element->id() == "ice-box-1" ) {
//							obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id, _iceBox1Texture->getId(), _iceBox1Texture->getWidth(), _iceBox1Texture->getHeight());
//						} else if ( element->id() == "stone-box-1" ) {
//							obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id, _stoneBox1Texture->getId(), _stoneBox1Texture->getWidth(), _stoneBox1Texture->getHeight());
//						} else if ( polygon->getInvMass() == 0.f) {
//	//                        obj.setVC(GL_TRIANGLE_STRIP, s_renderer->_unitQuadVertexDataVC);
//							obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id, _debugInfMassTexture->getId(), _debugInfMassTexture->getWidth(), _debugInfMassTexture->getHeight());
//						} else {
					{
	//                        obj.setVC(GL_TRIANGLE_STRIP, s_renderer->_unitQuadVertexDataVC);
							obj.setVT(GL_TRIANGLE_STRIP, s_renderer->getVtUnitQuadVertexData11Id(), _debugBoxTexture->getId(), _debugBoxTexture->getWidth(), _debugBoxTexture->getHeight());
						}
						break;
					case TrianglePolygonShape:
//						if ( element->id() == "wood-tri-1" ) {
//							obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id, _woodTri1Texture->getId(), _woodTri1Texture->getWidth(), _woodTri1Texture->getHeight());
//						} else if ( element->id() == "metal-tri-1" ) {
//							obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id, _metalTri1Texture->getId(), _metalTri1Texture->getWidth(), _metalTri1Texture->getHeight());
//						} else if ( element->id() == "ice-tri-1" ) {
//							obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id, _iceTri1Texture->getId(), _iceTri1Texture->getWidth(), _iceTri1Texture->getHeight());
//						} else if ( element->id() == "stone-tri-1" ) {
//							obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id, _stoneTri1Texture->getId(), _stoneTri1Texture->getWidth(), _stoneTri1Texture->getHeight());
//						} else {
					{
							obj.setVT(GL_TRIANGLE_STRIP, s_renderer->getVtUnitQuadVertexData11Id(), _debugTriTexture->getId(), _debugTriTexture->getWidth(), _debugTriTexture->getHeight());
						}
						break;
					default:
//						GLfloat* data = &polygon->localVertices()[0].X();
//						uint32 size = sizeof(Vector2<real>) * polygon->localVertices().size();
//						obj.setVerticesId(Renderer::createVbo(data, size, GL_ARRAY_BUFFER, GL_STATIC_DRAW));
						break;
				}
			} else {
				Disc* disc = Disc::cast(element);
				if ( disc ) {
//					if ( element->id() == "wood-disc-1" ) {
//						obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id,
//								  _woodDisc1Texture->getId(), _woodDisc1Texture->getWidth(), _woodDisc1Texture->getHeight());
//					} else if ( element->id() == "metal-disc-1" ) {
//						obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id,
//								  _metalDisc1Texture->getId(), _metalDisc1Texture->getWidth(), _metalDisc1Texture->getHeight());
//					} else if ( element->id() == "ice-disc-1" ) {
//						obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id,
//								  _iceDisc1Texture->getId(), _iceDisc1Texture->getWidth(), _iceDisc1Texture->getHeight());
//					} else if ( element->id() == "stone-disc-1" ) {
//						obj.setVT(GL_TRIANGLE_STRIP, s_renderer->_vtUnitQuadVertexData11Id,
//								  _stoneDisc1Texture->getId(), _stoneDisc1Texture->getWidth(), _stoneDisc1Texture->getHeight());
//					} else {
					{
						obj.setVT(GL_TRIANGLE_STRIP, s_renderer->getVtUnitQuadVertexData11Id(),
								  _debugDiscTexture->getId(), _debugDiscTexture->getWidth(), _debugDiscTexture->getHeight());
					}
				}
			}
			s_renderer->add(obj);
		}
	}
}

void callbackRenderObject(Renderer& /*renderer*/, uint32 type, RenderObject& object, Matrix4<real>& modelView)
{
	Element* element = (Element* )object.getUserData();
    if ( element ) {
		modelView = element->getModelViewMatrix();
    } else {
        switch (type) {
            case PROGRAM_VT:
            case PROGRAM_VTC:
				modelView = Matrix4<real>::makeScale((real)object.getTexWidth(), (real)object.getTexHeight(), 1);
                break;
            default:
                break;
        }
    }
}

void onLButtonDown(Window& win, int32 x, int32 y)
{
	Vector2<real> cx((real)x, (real)y);
	Vector4<real> wcx4 = screenToWorld(cx);
	Vector2<real> wcx(wcx4.X(), wcx4.Y());
	Element* element = s_world->elementContains(wcx);
	if ( !_touchJoint ) {
		if ( element ) {
			/*
			* attach pin joint to object
			*/
			Vector2<real> xLocal(element->getOwner()->toLocalSpace(wcx));
			Anchor a0(element->getOwner(), xLocal);
			Anchor a1(NULL, wcx);
			_touchJoint = PinJoint::create(a0, a1, 0.f, NULL);
			//_touchJoint->setSpring(5000.f, 0.f);
			s_world->attach(_touchJoint);
			_dragWorld = false;
		} else {
			/*
			* drag screen
			*/
			_dragScreenStart = Vector2<real>((real)x, (real)y);
			_dragWorldOffset = _worldOffset;
			_dragWorld = true;
			CONSOLEF("_dragWorldOffset = {%f, %f}\n", _dragWorldOffset.X(), _dragWorldOffset.Y());
			CONSOLEF("_dragScreenStart = {%f, %f}\n", _dragScreenStart.X(), _dragScreenStart.Y());
		}
	}
}
void onLButtonUp(Window& win, int32 x, int32 y)
{
    if (_touchJoint)
    {
        s_world->detach(_touchJoint);
        _touchJoint = NULL;
    }
    if ( _dragWorld ) {
        _dragWorld = false;
    }

}
void onRButtonDown(Window& win, int32 x, int32 y)
{

}
void onRButtonUp(Window& win, int32 x, int32 y)
{

}
void onKeyDown(Window& win, int32 k)
{

}
void onKeyUp(Window& win, int32 k)
{

}
void onCursorMove(Window& win, int32 x, int32 y)
{
	Vector2<real> wcx = screenToWorld(Vector2<real>((real)x, (real)y));
    if ( _touchJoint && !_dragWorld ) {
        _touchJoint->getAnchor1().setPoint(wcx);
        CONSOLEF("_touchJoint->getAnchor1().getPoint() = {%f, %f}\n", _touchJoint->getAnchor1().getPoint().X(), _touchJoint->getAnchor1().getPoint().Y());
    } else if ( _dragWorld ) {
        _worldOffset = _dragWorldOffset + (screenToWorld(_dragScreenStart) - wcx);
    }
    CONSOLEF("wcx = {%f, %f}\n", wcx.X(), wcx.Y());
}
